#include <ros/ros.h>
#include <stdio.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl/point_types.h> 
#include <tf/transform_broadcaster.h>
#include <QtExtSerialPort/qextserialport.h>
#include <QCoreApplication>
#include <QDebug>

QextSerialPort * port;
typedef unsigned char u8;

void sync_up()
{
    QByteArray active_response;
    // 等待10ms
    usleep(100000);
    port->readAll();      
}

int main(int argc, char ** argv)
{
	ros::init(argc, argv, "leo_driver");
	
	ros::NodeHandle nh;
	
	ros::Publisher leo_pointcloud_pub = 
	nh.advertise<sensor_msgs::LaserScan>("/base_scan", 10);

	QByteArray active_cmd_bytes;
	QByteArray sample_cmd_bytes;
	
	active_cmd_bytes[0] = 0xA5;
	active_cmd_bytes[1] = 0x5A;
	active_cmd_bytes[2] = 0x00;
	active_cmd_bytes[3] = 0x10;
	active_cmd_bytes[4] = 0x00;
	active_cmd_bytes[5] = 0x00;
	active_cmd_bytes[6] = 0x02;
	active_cmd_bytes[7] = 0x3A;
	active_cmd_bytes[8] = 0x00;
	active_cmd_bytes[9] = 0x01;
	active_cmd_bytes[10] = 0x00;
	active_cmd_bytes[11] = 0x00;
	active_cmd_bytes[12] = 0x00;
	active_cmd_bytes[13] = 0x00;
	active_cmd_bytes[14] = 0x00;
	active_cmd_bytes[15] = 0x00;
	active_cmd_bytes[16] = 0x00;
	active_cmd_bytes[17] = 0x00;
	active_cmd_bytes[18] = 0x00;
	active_cmd_bytes[19] = 0x00;
	active_cmd_bytes[20] = 0xFF;
	active_cmd_bytes[21] = 0xFF;
	
	sample_cmd_bytes[0] = 0xA5;
	sample_cmd_bytes[1] = 0x5A;
	sample_cmd_bytes[2] = 0x00;
	sample_cmd_bytes[3] = 0x10;
	sample_cmd_bytes[4] = 0x00;
	sample_cmd_bytes[5] = 0x00;
	sample_cmd_bytes[6] = 0x02;
	sample_cmd_bytes[7] = 0x3C;
	sample_cmd_bytes[8] = 0x00;
	sample_cmd_bytes[9] = 0x01;
	sample_cmd_bytes[10] = 0x00;
	sample_cmd_bytes[11] = 0x00;
	sample_cmd_bytes[12] = 0x00;
	sample_cmd_bytes[13] = 0x00;
	sample_cmd_bytes[14] = 0x00;
	sample_cmd_bytes[15] = 0x00;
	sample_cmd_bytes[16] = 0x00;
	sample_cmd_bytes[17] = 0x00;
	sample_cmd_bytes[18] = 0x00;
	sample_cmd_bytes[19] = 0x00;
	sample_cmd_bytes[20] = 0xFF;
	sample_cmd_bytes[21] = 0xFF;
	
	QString portName = QLatin1String("ttyUSB0");
	port = new QextSerialPort(QString(portName), QextSerialPort::EventDriven);
	
	port->setBaudRate(BAUD115200);
	port->setFlowControl(FLOW_OFF);
	port->setParity(PAR_NONE);
	port->setDataBits(DATA_8);
	port->setStopBits(STOP_1);
	
	if (port->open(QIODevice::ReadWrite) == true)
	{
		qDebug() << "listening for data on" << port->portName();
	}
	else 
	{
		qDebug() << "device failed to open:" << port->errorString();
	}
	
	int total = port->write(active_cmd_bytes, active_cmd_bytes.size());	
	qDebug() << total;
	
	sleep(1);
	
	int a = port->bytesAvailable();
	
	QByteArray active_response;
	if(a == 2)
	{
		active_response.resize(2);
		port->read(active_response.data(), active_response.size());
		
		if(active_response[0] == 0x00 && active_response[1] == 0x01)
		{
			std::cout << "device active success" << std::endl;
		}
	}
	
	const int num_readings = 224;
	
	sensor_msgs::LaserScan scan;
	
	scan.header.frame_id = "/laser";
	scan.angle_min = - 56 * (3.1415926 / 180);
	scan.angle_max = 56 * (3.1415926 / 180);
	scan.angle_increment = 0.5 * (3.14 /180);
	scan.range_min = 0.0;
	scan.range_max = 2.0;
	
	scan.ranges.resize(224);
	
	ros::Rate loop_rate(5);
	
	while(ros::ok())
	{
		//等待300ms，采样频率为3Hz
		loop_rate.sleep();
		
		//采集数据
		port->flush();
		
		int total = port->write(sample_cmd_bytes, sample_cmd_bytes.size());	
		usleep(100000);
		
		//处理数据
		QByteArray sample_data;
		
		int read_cnt = port->bytesAvailable();
		
		QByteArray response_header;
		
		if(read_cnt >= 2)
		{
			response_header.resize(2);
			port->read(response_header.data(), 2);
		}
		
		if(response_header[0] == 0xA5 && response_header[1] == 0x01)
		{
			std::cout << "sample data ok" << std::endl;
		}
		else
        {
            std::cout <<  "sample not ok" <<  std::endl;
             sync_up();
             continue;
        }
		
		QByteArray onebyte;
		sample_data.resize(448);
		
		for(int i = 0 ; i < 448; i++)
		{
			while(port->bytesAvailable() == 0)
			{
				usleep(10000);
			}
			
			port->read(onebyte.data(), 1);
			
			sample_data[i] = onebyte[0];
		}
		
		for(int i = 0; i < 448; i = i+2)
		{
			scan.ranges[223 - i/2] = 1.0 * ((u8)sample_data[i] * 255 + (u8)sample_data[i+1])/1000;
		}
		
		leo_pointcloud_pub.publish(scan);
		
		static tf::TransformBroadcaster br;
		tf::Transform transform;
		
		transform.setOrigin(tf::Vector3(0.13, 0, 0));
		
		tf::Quaternion q; 
		q.setRPY(0, 0, 0);
		
		transform.setRotation(q);
		
		br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
											  "/base_link", "/laser"));
											  
        //br.sendTransform(tf::StampedTransform(transform,  ros::Time::now(), 
        //                                        "/base_link", "laser"));
	}
	
	return 0; 
}
